#ifndef PERCEPTION_Perception_H
#define PERCEPTION_Perception_H

#include <chrono>
#include <functional>
#include <iostream>
#include <string>

// 第三方库
// #include "common/log.h"

// ros
#include "rclcpp/rclcpp.hpp"
#include <geometry_msgs/msg/point.hpp>
#include <nav_msgs/msg/grid_cells.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

// proto
#include "chassis.pb.h"
#include "localization.pb.h"
#include "msg_interfaces/msg/matrix_proto.hpp"
#include "perception_common.pb.h"
#include "perception_obstacle.pb.h"

//
#include "../../architecture/lidar_cells/lidar_cells.h"
#include "../../architecture/preprocess/preprocess.h"
#include "common/data_pool.h"
#include "readconfig.h"
namespace perception
{
    using std::placeholders::_1;
    using namespace std::chrono_literals;
    class Perception : public rclcpp::Node
    {
    private:
        rclcpp::TimerBase::SharedPtr timer_;

        rclcpp::SubscriptionOptions options;
        rclcpp::CallbackGroup::SharedPtr callBackGroup_;
        rclcpp::Publisher<msg_interfaces::msg::MatrixProto>::SharedPtr lidar_objs_pub;
        rclcpp::Publisher<nav_msgs::msg::GridCells>::SharedPtr lidar_cells_pub;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_pre_pub;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_pre_pub2;

        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_sub0;
        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_sub1;
        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_sub2;
        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_sub3;
        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_sub4;
        rclcpp::Subscription<msg_interfaces::msg::MatrixProto>::SharedPtr loc_sub;
        rclcpp::Subscription<msg_interfaces::msg::MatrixProto>::SharedPtr vs_sub;

        // sub topic name
        std::string lidar_topic_name0 = "lidar_point0";
        std::string lidar_topic_name1 = "lidar_point1";
        std::string lidar_topic_name2 = "lidar_point2";
        std::string lidar_topic_name3 = "lidar_point3";
        std::string lidar_topic_name4 = "lidar_point4";
        std::string loc_topic = "/localization";
        std::string vs_topic = "/vehicle_state";
        // pub topic name
        std::string pub_pre_topic = "/lidar_perception/prepointcloud";
        std::string pub_pre_topic2 = "/lidar_perception/prepointcloud2";
        std::string pub_cells_topic = "/lidar_perception/lidar_cells";
        std::string pub_objs_topic = "/lidar_perception/lidar_objs";

        int hz = 10;

        std::shared_ptr<ReadConfig> readconfig_;
        std::string cell_config_path_;
        std::string preprocess_config_path_;
        std::string obj_config_path_;

        std::shared_ptr<perception::lidar_preprocess::PreProcess> preprocess_;
        std::shared_ptr<perception::algorithm::LidarCells> lidar_cell_;

        void CallbackLocalization(const msg_interfaces::msg::MatrixProto::SharedPtr msg);
        void CallbackVehicleState(const msg_interfaces::msg::MatrixProto::SharedPtr msg);
        void CallbackLidar(const sensor_msgs::msg::PointCloud2::SharedPtr msg, int lidar_num);
        void TimerCallback();
        void Publish();
        void PubObjs();
        void InitSubPub();
        void InitParams();
        void InitAlgorithm();
        bool Process();
        void Init();

    public:
        Perception(/* args */);
        ~Perception();
    };

    // } //lidar_cells
} // perception

#endif